/*****************************************************************************

*This software is developed within IEEE 1857.9 subgroup for the reference test model of Immersive Video Content Coding.
*Contributor: Yueming Wang, wymmail@foxmail.com, Peking University Shenzhen Graduate School

*****************************************************************************/
#define _USE_MATH_DEFINES
#include "stdafx.h"
#include <stdlib.h>
#include <math.h>
#include "global.h"

#define LANCZOS_TAB_SIZE        3
#define LANCZOS_FAST_SCALE      100
#define LANCZOS_FAST_MAX_SIZE   4096

static double tbl_lanczos_coef[LANCZOS_FAST_MAX_SIZE * LANCZOS_FAST_SCALE];
#define lanczos_coef(x) tbl_lanczos_coef[(int)(fabs(x) * LANCZOS_FAST_SCALE + 0.5)]

__inline float linearinterpolation(float a, float b, float k){
	return a * (1.0f - k) + b * k;
}


__inline float cubicinterpolation(float aZ, float a0, float a1, float a2, float t){
	float cZ = 2 * a0;
	float c0 = -aZ + a1;
	float c1 = 2 * aZ - 5 * a0 + 4 * a1 - a2;
	float c2 = -aZ + 3 * a0 - 3 * a1 + a2;

	float t2 = t * t;
	float t3 = t2 * t;

	float v = 0.5f*(cZ + c0*t + c1*t2 + c2*t3);
	return clip(v, 0, 255.0f);
}

float filter_cubic(unsigned char * src, float i, float j, int input_width, int input_height, int i_src){
	const int width_LB = 1;
	const int width_HB = input_width - 2;
	const int height_LB = 1;
	const int height_HB = input_height - 2;

	if ((i <= height_LB) || (i >= height_HB) || (j <= width_LB) || (j >= width_HB)){
		// linear filter 
		i = clip(i, 0.0f, input_height - 1.0f);
		j = clip(j, 0.0f, input_width - 1.0f);
		{
		const int jl = (int)floor(j);
		const int jh = (int)ceil(j);
		const int il = (int)floor(i);
		const int ih = (int)ceil(i);
		return (linearinterpolation(
			   linearinterpolation(*(src + il * i_src + jl), *(src + il * i_src + jh), j - jl),
			   linearinterpolation(*(src + ih * i_src + jl), *(src + ih * i_src + jh), j - jl),
			   i-il) + 0.5f);
		}
	}
	else{
		const long j0 = (long)floorf(floorf(j) - 1);
		const long j1 = j0 + 1;
		const long j2 = j1 + 1;
		const long j3 = j2 + 1;
		const long i0 = (long)floorf(floorf(i) - 1);
		const long i1 = i0 + 1;
		const long i2 = i1 + 1;
		const long i3 = i2 + 1;

		return (cubicinterpolation(
			   cubicinterpolation(*(src + i0 * i_src + j0),
								  *(src + i0 * i_src + j1),
								  *(src + i0 * i_src + j2),
								  *(src + i0 * i_src + j3), j - j1),
			   cubicinterpolation(*(src + i1 * i_src + j0),
								  *(src + i1 * i_src + j1),
								  *(src + i1 * i_src + j2),
								  *(src + i1 * i_src + j3), j - j1),
			   cubicinterpolation(*(src + i2 * i_src + j0),
								  *(src + i2 * i_src + j1),
								  *(src + i2 * i_src + j2),
								  *(src + i2 * i_src + j3), j - j1),
			   cubicinterpolation(*(src + i3 * i_src + j0),
								  *(src + i3 * i_src + j1),
								  *(src + i3 * i_src + j2),
								  *(src + i3 * i_src + j3), j - j1),
								  i - i1) + 0.5f);
	}
}

static double sinc(double x)
{
	x *= M_PI;
	if (x < 0.01 && x > -0.01) {
		double x2 = x * x;
		return 1.0f + x2 * (-1.0 / 6.0 + x2 / 120.0);
	}
	else {
		return sin(x) / x;
	}
}

void init_lanczos_filter()
{
	int i;
	for (i = 0; i<LANCZOS_FAST_MAX_SIZE*LANCZOS_FAST_SCALE; i++)
	{
		double x = (double)i / LANCZOS_FAST_SCALE;
		tbl_lanczos_coef[i] = sinc(x) * sinc(x / LANCZOS_TAB_SIZE);
	}
}

float filtre_lanczos(unsigned char * src, float j, float i, int input_width, int input_height, int i_src)
{
	double coef, sum = 0, res = 0;
	int m, n, idx_x, idx_y;
	float ret_val = 0;

	for (n = -LANCZOS_TAB_SIZE; n<LANCZOS_TAB_SIZE; n++)
	{
		for (m = -LANCZOS_TAB_SIZE; m<LANCZOS_TAB_SIZE; m++)
		{
			idx_x = (int)i + m + 1;
			idx_y = (int)j + n + 1;

			coef = lanczos_coef(i - idx_x) * lanczos_coef(j - idx_y);

			// when the neib. pixel is outside the boundary, using the boundary pixels
			idx_x = (idx_x < 0) ? 0 : idx_x;
			idx_y = (idx_y < 0) ? 0 : idx_y;
			idx_x = (idx_x >= input_width) ? (input_width - 1) : idx_x;
			idx_y = (idx_y >= input_height) ? (input_height - 1) : idx_y;

			res += src[idx_x + idx_y * i_src] * coef;
			sum += coef;
		}
	}

	if (sum != 0) {
		ret_val = (float)(res / sum + 0.5);
		ret_val = clip(ret_val, 0.0f, 255.0f);
	}

	return ret_val;
}

void init_function_filter(sourceInfo *input_Info, sourceInfo *output_Info){
	handle_function.init_lanczos_filter = init_lanczos_filter;
	handle_function.filter = filtre_lanczos;
}